#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

cv::Mat get_gaussian_kernel() {
    cv::Mat gk = (cv::Mat_<double>(5, 5) <<
        1, 4, 6, 4, 1,
        4, 16, 24, 16, 4,
        6, 24, 36, 24, 6,
        4, 16, 24, 16, 4,
        1, 4, 6, 4, 1);
    gk /= 256;
    return gk;
}

cv::Mat gaussian_pyramid_downsampling(const cv::Mat& mat) {
    CV_Assert(mat.type() == CV_8UC1);
    
    cv::Mat gk = get_gaussian_kernel();
    std::cout << "gk = \n" << gk << std::endl << std::endl;
    cv::Mat matPadded;
    cv::copyMakeBorder(mat, matPadded, gk.rows / 2, gk.rows / 2, gk.cols / 2, gk.cols / 2, cv::BORDER_REFLECT101);
    cv::Mat conv(mat.size(), CV_8UC1);
    for (int i = 0; i < mat.rows; i++) {
        uchar* p = conv.ptr<uchar>(i);
        for (int j = 0; j < mat.cols; j++) {
            cv::Mat roi(matPadded, cv::Rect(j, i, gk.cols, gk.rows));
            cv::Mat roiDouble;
            roi.convertTo(roiDouble, CV_64FC1);
            cv::Mat res;
            cv::multiply(roiDouble, gk, res);
            cv::Scalar s = cv::sum(res);
            p[j] = cv::saturate_cast<uchar>(s[0]);
        }
    }
    cv::Mat result((conv.rows+1) / 2, (conv.cols+1) / 2, CV_8UC1);
    for (int i = 0; i < result.rows; i++) {
        uchar* p = result.ptr<uchar>(i);
        uchar* pp = conv.ptr<uchar>(i*2);
        for (int j = 0; j < result.cols; j++) {
            p[j] = pp[j*2];
        }
    }
    return result;
}

cv::Mat laplacian_pyramid_upsampling(const cv::Mat& mat) {
    CV_Assert(mat.type() == CV_8UC1);
    
    cv::Mat res = cv::Mat::zeros(mat.rows*2, mat.cols*2, CV_8UC1);
    for (int i = 0; i < mat.rows; i++) {
        const uchar* p = mat.ptr<uchar>(i);
        uchar* pp = res.ptr<uchar>(i*2);
        for (int j = 0; j < mat.cols; j++) {
            pp[j*2] = p[j];
        }
    }
    
    cv::Mat gk = get_gaussian_kernel();
    gk *= 4;
    std::cout << "gk = \n" << gk << std::endl << std::endl;
    cv::Mat resPadded;
    cv::copyMakeBorder(res, resPadded, gk.rows / 2, gk.rows / 2, gk.cols / 2, gk.cols / 2, cv::BORDER_REFLECT101);
    for (int i = 0; i < res.rows; i++) {
        uchar* p = res.ptr<uchar>(i);
        for (int j = 0; j < res.cols; j++) {
            cv::Mat roi(resPadded, cv::Rect(j, i, gk.cols, gk.rows));
            cv::Mat roiDouble;
            roi.convertTo(roiDouble, CV_64FC1);
            cv::Mat mul;
            cv::multiply(roiDouble, gk, mul);
            cv::Scalar s = cv::sum(mul);
            p[j] = cv::saturate_cast<uchar>(s[0]);
        }
    }
    return res;
}

int main(int argc, char **argv) {
    /*
    cv::Mat mat(16, 19, CV_8UC1);
    cv::randu(mat, cv::Scalar(0), cv::Scalar(255));
    cv::Mat downsample;
    cv::pyrDown(mat, downsample, cv::Size(), cv::BORDER_REFLECT101);
    std::cout << "downsample = \n" << downsample << std::endl << std::endl;
    cv::Mat myDownsample = gaussian_pyramid_downsampling(mat);
    std::cout << "myDownsample = \n" << myDownsample << std::endl << std::endl;
    cv::Mat downsampleDiff = myDownsample - downsample;
    std::cout << "downsampleDiff = \n" << downsampleDiff << std::endl << std::endl;
    std::vector<cv::Point> nonzeros;
    cv::findNonZero(downsampleDiff, nonzeros);
    std::cout << "nonzeros = \n" << nonzeros << std::endl << std::endl;
    */
    
    cv::Mat lena = cv::imread("/home/xlll/Downloads/opencv/samples/data/lena.jpg", cv::IMREAD_GRAYSCALE);
    if (lena.empty()) {
        std::cout << "Failed to read image!" << std::endl;
        return EXIT_FAILURE;
    }
    /*
    cv::Mat lenaDown;
    cv::pyrDown(lena, lenaDown, cv::Size(), cv::BORDER_REFLECT101);
    cv::imshow("lena", lena);
    cv::imshow("lenaDown", lenaDown);
    cv::waitKey(0);
    */
    
    cv::Mat mat(4, 5, CV_8UC1);
    cv::randu(mat, cv::Scalar(0), cv::Scalar(255));
    cv::Mat upsample;
    cv::pyrUp(mat, upsample, cv::Size(), cv::BORDER_REFLECT101);
    std::cout << "upsample = \n" << upsample << std::endl << std::endl;
    cv::Mat myUpsample = laplacian_pyramid_upsampling(mat);
    std::cout << "myUpsample = \n" << myUpsample << std::endl << std::endl;
    cv::Mat upsampleDiff = myUpsample - upsample;
    std::cout << "upsampleDiff = \n" << upsampleDiff << std::endl << std::endl;
    std::vector<cv::Point> nonzeros;
    cv::findNonZero(upsampleDiff, nonzeros);
    std::cout << "nonzeros = \n" << nonzeros << std::endl << std::endl;
    
    cv::Mat lenaUp;
    cv::pyrUp(lena, lenaUp, cv::Size(), cv::BORDER_REFLECT101);
    cv::imshow("lena", lena);
    cv::imshow("lenaUp", lenaUp);
    cv::waitKey(0);
}
